#include "rosnode.h"
#include <ros/ros.h>
#include <ros/network.h>
#include <std_msgs/UInt8MultiArray.h>

#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <image_transport/image_transport.h>
#include <geometry_msgs/Twist.h>

#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <QDebug>

QString RosNode::s_strLog;
QString RosNode::s_LogStr;
cv::Mat RosNode::s_Mat;
bool RosNode::s_bIsRosTriggerUiUpdate = false;
bool RosNode::s_bIsUiUpdateFinished = true;
bool RosNode::s_bIsRosTriggerMatUpdate = true;
bool RosNode::s_bIsMatUpdateFinished = false;

RosNode::RosNode(int argc,char **argv) : rosArgc(argc), ppRosArgv(argv)
{
    rosFrequency = 10;
}

RosNode::~RosNode()
{
    if(ros::isStarted())
    {
        ros::shutdown(); // explicitly needed since we use ros::start();
        ros::waitForShutdown();
    }
    wait();
}

bool RosNode::init()
{
    if(false == ros::isInitialized())
    {
        ros::init(rosArgc,ppRosArgv,"RosNode");
    }
    if ( ! ros::master::check() )
    {
        return false;
    }
    ros::start();
    rosLog(INFO,"Ros node without marster_uri init success!");
    start();
    return true;
}

bool RosNode::init(const std::string &master_url, const std::string &host_url)
{
    if(false == ros::isInitialized())
    {
        std::map<std::string, std::string> remappings;
        remappings["__master"] = master_url;
        remappings["__hostname"] = host_url;
        ros::init(remappings, "RosNode");
    }

    if ( ! ros::master::check() )
    {
        return false;
    }
    ros::start();
    rosLog(INFO,"Ros node with marster_uri init success!");
    start();
    return true;
}

void RosNode::run()
{
    ros::NodeHandle stNodeHandle;
    image_transport::ImageTransport it(stNodeHandle);
    image_transport::Subscriber sub = it.subscribe( "gtz_image", 1, imageCallback, image_transport::TransportHints("compressed"));
    stClusterMsgSub = stNodeHandle.subscribe<std_msgs::UInt8MultiArray>("/CommClusterMsg", CLUSTER_MAX_NODE_NUM * 3, &RosNode::fnCluster_mh_ClusterMsgCB, this);
    stClusterMsgPub = stNodeHandle.advertise<std_msgs::UInt8MultiArray>("/CommClusterMsg", CLUSTER_MAX_NODE_NUM * 3);

    stClusterDediMsgSub = stNodeHandle.subscribe<std_msgs::UInt8MultiArray>("/uav0/DediClusterMsg", CLUSTER_MAX_NODE_NUM * 3, &RosNode::fnCluster_mh_ClusterMsgCB, this);
    stNodeHandle.setParam("gtz_image/compressed/jpeg_quality",20);

    rosLog(INFO,"spin start!");
    ros::Rate loopRate(rosFrequency);
    freqDividerCnt = 0;
    while(ros::ok())
    {
        ros::spinOnce();
        if(0 != s_LogStr.size())
        {
            rosLog(INFO, s_LogStr.toStdString());
            s_LogStr.clear();
        }

        freqDividerCnt++;

        if(true == s_bIsRosTriggerMatUpdate)
        {
            s_bIsRosTriggerMatUpdate = false;
            s_bIsMatUpdateFinished = false;
            Q_EMIT signalMatUpdate(s_Mat);
        }
        if(0 == (freqDividerCnt % rosFrequency))
        {
            Q_EMIT signalLogUpdated();
        }

        if(true == s_bIsRosTriggerUiUpdate)
        {
        //    s_bIsUiUpdateFinished = false;
            s_bIsRosTriggerUiUpdate = false;
            Q_EMIT signalRosTriggerUiUpdate();
        }

        loopRate.sleep();
    }
    rosLog(INFO,"spin end!");
}

void RosNode::slotRosActionHandle()
{
    QString strTemp;
    //cUint32 uMsgLen;
    //cUint8 aMsgArray[1024];
    std_msgs::UInt8MultiArray pubStream;

    switch(stQgcSendMsg.stMsgHeader.uMsgID)
    {
    case CLUSTER_QGC_CONFIG_TARGET_INFO:
    {
        stQgcSendMsg.stMsgHeader.uTimeStamp = 0xff;
        stQgcSendMsg.stMsgHeader.uMsgLength = sizeof(Cluster_Msg_Header) + sizeof(QGC_Config_Target_Info);
        fnCluster_mh_StructToArray(pubStream, (cInt8*)(&stQgcSendMsg.stMsgHeader), sizeof(Cluster_Msg_Header));
        fnCluster_mh_StructToArray(pubStream, (cInt8*)(&stQgcSendMsg.stConfigTargetInfo), sizeof(QGC_Config_Target_Info));
        break;
    }
    case CLUSTER_QGC_CONFIG_FLIGHT_INFO:
    {
        stQgcSendMsg.stMsgHeader.uTimeStamp = 0xff;
        stQgcSendMsg.stMsgHeader.uMsgLength = sizeof(Cluster_Msg_Header) + sizeof(QGC_Config_Flight_Info);
        fnCluster_mh_StructToArray(pubStream, (cInt8*)(&stQgcSendMsg.stMsgHeader), sizeof(Cluster_Msg_Header));
        fnCluster_mh_StructToArray(pubStream, (cInt8*)(&stQgcSendMsg.stConfigFlightInfo), sizeof(QGC_Config_Flight_Info));
        break;
    }
    case CLUSTER_QGC_CMD_SELF_CHECK:
    case CLUSTER_QGC_CMD_ACTIVE_POWER:
    case CLUSTER_QGC_CMD_TAKEOFF:
    case CLUSTER_QGC_CMD_STANDBY:
    case CLUSTER_QGC_CMD_RETURN_PICTURE:
    case CLUSTER_QGC_CMD_LAND:
    {
        stQgcSendMsg.stMsgHeader.uTimeStamp = 0xff;
        stQgcSendMsg.stMsgHeader.uMsgLength = sizeof(Cluster_Msg_Header);
        fnCluster_mh_StructToArray(pubStream, (cInt8*)(&stQgcSendMsg.stMsgHeader), sizeof(Cluster_Msg_Header));
        break;
    }
    default:
    {
        rosLog(ERROR, "Send cmd with error msg id!");
        return;
    }
    }

    //stEncodeMsg.pastMessageCntnt[0] = (Cluster_Message_Content *)&operatorMsg;
    //uMsgLen = fnCluster_codec_EncodeMessage(aMsgArray, &stEncodeMsg);
    //fnCluster_codec_FourByteToStream(aMsgArray, &uSendSn);


    //qDebug() << msgLen;
    //s_strLog.append(strTemp.sprintf("length = %x, content: ",uMsgLen));
    //if(uMsgLen > 1000)
    {
    //    rosLog(WARN, "Encode Msg Len > 1000,equal to "+ std::to_string(uMsgLen));
        //assert(0);
    }

    s_LogStr.append(PRINT_HEADER+QString::asprintf("Send Binary[%x]:",stQgcSendMsg.stMsgHeader.uMsgLength));
    for(cUint32 i = 0; i < stQgcSendMsg.stMsgHeader.uMsgLength; i++)
    {
        //pubStream.data.push_back(aMsgArray[i]);
        //s_strLog.append(strTemp.sprintf("%x,",pubStream.data[i]));
        s_LogStr.append(QString::asprintf("%x,",pubStream.data[i]));
    }
    //s_strLog.append("\n");
    //qDebug() << strLog;

    stClusterMsgPub.publish(pubStream);
}

void RosNode::fnCluster_mh_ClusterMsgCB(const std_msgs::UInt8MultiArray::ConstPtr &msg)
{
    QString strTemp;
    cUint8 aMsgStream[1024];
    cUint32 uMsgLen = (cUint32)msg->data.size();
    Cluster_Msg_Header *pstHeader = nullptr;
    //qDebug() << "interfaceMsgCallback";

    //QGC_Config_Target_Info *pstCfgTatgetInfo = nullptr;

    //QGC_Config_Flight_Info  *pstCfgFlightInfo = nullptr;

    if(uMsgLen > 1000)
    {
        //s_strLog.append("Encode Msg Len > 1000,equal to ");
        //s_strLog.append(strTemp.sprintf("%x",uMsgLen));
        return;
    }
    //s_strLog.append(strTemp.sprintf("length = %x, content: ",uMsgLen));

    for(cUint32 i = 0; i < uMsgLen; i++)
    {
        aMsgStream[i] = msg->data[i];
        //s_strLog.append(strTemp.sprintf("%x,",aMsgStream[i]));
    }

    /*s_LogStr.append(PRINT_HEADER+QString::asprintf("Receive Binary[%x]:",uMsgLen));
    for(cUint32 i = 0; i < uMsgLen; i++)
    {
        s_LogStr.append(QString::asprintf("%x,",aMsgStream[i]));
    }*/

    pstHeader = (Cluster_Msg_Header *)aMsgStream;

    switch(pstHeader->uMsgID)
    {
    case CLUSTER_QGC_CONFIG_TARGET_INFO:
    /*{
        if(pstHeader->uMsgLength == sizeof(Cluster_Msg_Header) + sizeof(QGC_Config_Target_Info))
        {
            pstCfgTatgetInfo = (QGC_Config_Target_Info *)&aMsgStream[sizeof(Cluster_Msg_Header)];
        }
        break;
    }*/
    case CLUSTER_QGC_CONFIG_FLIGHT_INFO:
    /*{
        if(pstHeader->uMsgLength == sizeof(Cluster_Msg_Header) + sizeof(QGC_Config_Flight_Info))
        {
            pstCfgFlightInfo = (QGC_Config_Flight_Info *)&aMsgStream[sizeof(Cluster_Msg_Header)];
        }
        break;
    }*/
    case CLUSTER_QGC_CMD_SELF_CHECK:
    case CLUSTER_QGC_CMD_ACTIVE_POWER:
    case CLUSTER_QGC_CMD_TAKEOFF:
    case CLUSTER_QGC_CMD_STANDBY:
    case CLUSTER_QGC_CMD_RETURN_PICTURE:
    case CLUSTER_QGC_CMD_LAND:
    {
        s_LogStr.append(PRINT_HEADER+QString::asprintf("Receive Qgc Msg"));
        break;
    }
    case CLUSTER_UAV_STATE_REPORT:
    {
        if(pstHeader->uMsgLength == sizeof(Cluster_Msg_Header) + sizeof(Uav_State_To_Qgc))
        {
            stQgcSaveMsg.stUavInfo[pstHeader->uSourceID - 1] = *((Uav_State_To_Qgc *)&aMsgStream[sizeof(Cluster_Msg_Header)]);
            stQgcSaveMsg.auMapArray[pstHeader->uSourceID - 1] = true;
            //memcpy(&stQgcSaveMsg.stUavInfo[pstHeader->uSourceID - 1], &aMsgStream[sizeof(Cluster_Msg_Header)], sizeof(Uav_State_To_Qgc));
        }
        //s_LogStr.append(QString::asprintf("stateus x= %lf,y = %lf,",stQgcSaveMsg.stUavInfo[pstHeader->uSourceID - 1].uLongitude,stQgcSaveMsg.stUavInfo[pstHeader->uSourceID - 1].uLatitude));
        break;
    }
    case CLUSTER_UAV_TARGET_SHARE:
    {
        if(pstHeader->uMsgLength == sizeof(Cluster_Msg_Header) + sizeof(Uav_Target_To_Qgc))
        {
            stQgcSaveMsg.stTargetInfo = *((Uav_Target_To_Qgc *)&aMsgStream[sizeof(Cluster_Msg_Header)]);
        }
        break;
    }
    default:
    {
        s_LogStr.append(QString::asprintf("%x,",1));
        return;
    }
    }
#if 0
    //s_strLog.append("\n");
    fnCluster_codec_DecodeMessage(aMsgStream, uMsgLen, &stDecodeMsg);
    //qDebug() << s_stDecodeMsg.aGlobalPos[0];

    if((NULL == stDecodeMsg.pastMessageCntnt[0]) || (CLUSTER_QGC_ID == stDecodeMsg.pastMessageCntnt[0]->stStateMessage.stMsgHeader.uSendID))
    {
        return;
    }

    pstClusterCntxt->uMainNodeID = stDecodeMsg.pastMessageCntnt[0]->stStateMessage.stMsgHeader.uMainNodeID;

    for(int i = 0; i < CLUSTER_MAX_MSG_CONTAINER_NUM; i++)
    {
        if(CLUSTER_MSG_TYPE_STATE == stDecodeMsg.aenClusterMsgType[i])
        {
            fnCluster_mh_SaveStateMessage(pstClusterCntxt, &stDecodeMsg.pastMessageCntnt[i]->stStateMessage);
        }
        else if(CLUSTER_MSG_TYPE_CONTROL == stDecodeMsg.aenClusterMsgType[i])
        {
            fnCluster_mh_SaveControlMessage(pstClusterCntxt, &stDecodeMsg.pastMessageCntnt[i]->stControlMessage);
        }

        if(CLUSTER_MSG_TYPE_NONE != stDecodeMsg.aenClusterMsgType[i])
        {
            stDecodeMsg.aenClusterMsgType[i] = CLUSTER_MSG_TYPE_NONE;
            fnCluster_buf_FreeBuf((cUint8 *)stDecodeMsg.pastMessageCntnt[i]);
            stDecodeMsg.pastMessageCntnt[i] = NULL;
        }
    }

    pstClusterCntxt->uSubNodeNum = 0;
    for(int i = 0; i < CLUSTER_MAX_NODE_NUM; i++)
    {
        if(CLUSTER_NETWORK_STATE_NONE != pstClusterCntxt->astSubNodeState[i].enClusterNetworkState)
        {
            pstClusterCntxt->uSubNodeNum++;
        }
    }
#endif
    if(true == s_bIsUiUpdateFinished)
    {
        s_bIsRosTriggerUiUpdate = true;
    }
}

void RosNode::imageCallback(const sensor_msgs::ImageConstPtr &msg)
{
    try{
        if(true == s_bIsMatUpdateFinished)
        {
            s_Mat = cv_bridge::toCvShare(msg, "bgr8")->image.clone();
            //qDebug() << s_Mat.rows;
            s_bIsRosTriggerMatUpdate = true;
            //cv::imshow( "video",  srcMat);
        }
    }
    catch( cv_bridge::Exception& e )
    {
        ROS_ERROR( "Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str() );
    }
}

void RosNode::rosLog( const LogLevel &level, const std::string &msg) {
    log_model.insertRows(log_model.rowCount(),1);
    std::stringstream logging_model_msg;

    if(false != ros::isInitialized())
    {
        logging_model_msg << "[" << ros::Time::now() << "] ";
    }
    switch ( level )
    {
        case(DEBUG):
        {
            //ROS_DEBUG_STREAM(msg);
            logging_model_msg << "[DEBUG]: " << msg;
            break;
        }
        case(INFO):
        {
            //ROS_INFO_STREAM(msg);
            logging_model_msg << "[INFO]: " << msg;
            break;
        }
        case(WARN):
        {
            //ROS_WARN_STREAM(msg);
            logging_model_msg << "[WARN]: " << msg;
            break;
        }
        case(ERROR):
        {
            //ROS_ERROR_STREAM(msg);
            logging_model_msg << "[ERROR]: " << msg;
            break;
        }
        case(FATAL):
        {
            //ROS_FATAL_STREAM(msg);
            logging_model_msg << "[FATAL]: " << msg;
            break;
        }
    }
    QVariant new_row(QString(logging_model_msg.str().c_str()));
    log_model.setData(log_model.index(log_model.rowCount()-1),new_row);
}

void RosNode::slotRosClose()
{
    if(ros::isStarted())
    {
        rosLog(INFO,"ros close start!");
        //qDebug() << "ros close";
        ros::shutdown();
        ros::waitForShutdown();
        rosLog(INFO,"ros closed!");
    }
}
